#include <inttypes.h>
#include "packet.h"
#include "netcom.h"
#include "pms5005.h"
#include "pmb5010.h"
#include "trace.h"

status_t robot_data_set(packet_t *packet, robot_data_t *data)
{
//	TRACE("BEGIN robot_data_set");
	uint8_t i = 0;

	if ( NULL == packet || NULL == data ) {
		return NULL_POINTER;
	}

	switch ( packet->did ) {
		case MOT_FB:
			for ( i = 0; i < 7; i++ ) {
				data->potentiometer[i] = packet->data[i*2] +
						(packet->data[i*2+1] << 8);
				if ( i == 6 ) 
					continue; //we'll skip the mot6
				data->motor_feedback[i] = (double)
					(packet->data[i*2+12] +
					(packet->data[i*2+13] << 8)) / 728.0;
			}

			data->encoder_count[0] = packet->data[24] + 
						(packet->data[25] << 8);
			data->encoder_count[1] = packet->data[28] + 
						(packet->data[29] << 8);
			data->encoder_speed[0] = packet->data[26] +
						(packet->data[27] << 8);
			data->encoder_speed[1] = packet->data[30] +
						(packet->data[31] << 8);
			data->encoder_dir[0] = 0x01 & packet->data[32];
			data->encoder_dir[1] = 0x02 & packet->data[32];
			break;
		case CUSTOM_AD:
			for (i = 0; i<8; i++) {
				data->custom_ad[i] = packet->data[2*i] +
						(packet->data[2*i+1] << 8);
			}
			data->custom_io = packet->data[16]; 
			break;
		case SENSOR_FB:
			for (i = 0; i<6; i++) {
				data->us_range[i] = packet->data[i];
			}
			data->human_alarm[0] = packet->data[6] + 
					(packet->data[7] << 8);
			data->human_alarm[1] = packet->data[10] + 
					(packet->data[11] << 8);
			data->human_detect[0] = packet->data[8] +
					(packet->data[9] << 8);
			data->human_detect[1] = packet->data[12] +
					(packet->data[13] << 8);
			data->ir_range = packet->data[24] + 
					(packet->data[25] << 8);
			data->mainboard_pw = packet->data[30] + 
					(packet->data[31] << 8);
			data->dc_motor_pw = packet->data[32] +
					(packet->data[33] << 8);
			
			break;
		case JPEG_PACKET :
			break;
	}

//	TRACE("END robot_data_set");
	return SUCCESS;
}


static void calculate_movement_offset(uint16_t *pos1, 
					uint16_t *pos2, 
					double dist)
{
	uint16_t diff = dist/51.83627842 * 1200.0;
	uint16_t max_16 = 32767;
	int16_t p1 = *pos1, p2 = *pos2;

	if ( dist > 0 ) {
		p1 += diff;
		p2 -= diff;

		if ( p1 > 0 ) {
			p1 -= max_16;
		}

		if ( p2 < 0 ) {
			p2 += max_16;
		}
	} else {
		p1 += diff;
		p2 -= diff;

		if ( p1 < 0 ) {
			p1 += max_16;
		}

		if ( p2 > 0 ) {
			p2 -= max_16;
		}
	}

	*pos1 = p1;
	*pos2 = p2;
}

static void calculate_rotation_offset(uint16_t *pos1, 
					uint16_t *pos2, 
					double degr)
{
	uint16_t diff = (((degr/180) * 3.1415 * 0.265) / 2 / 0.5183) * 1200;
	uint16_t max_16 = 32767;
	int16_t p1 = *pos1, p2 = *pos2;

	p1 += diff;
	p2 += diff;

	if ( p1 < 0 ) {
		p1 += max_16;
	} else if ( p1 > max_16 ) {
		p1 -= max_16;
	}

	if ( p2 < 0 ) {
		p2 += max_16;
	} else if ( p1 > max_16 ) {
		p2 -= max_16;
	}

	*pos1 = p1;
	*pos2 = p2;
}

packet_t *robot_move(robot_data_t *data, double dist)
{
	packet_t *packet = NULL;
	uint16_t pos1, pos2;

    if ( NULL == data ) {
        return NULL_POINTER;
    }
    
	pos1 = data->encoder_count[0];
	pos2 = data->encoder_count[1];

	calculate_movement_offset(&pos1, &pos2, dist);
	packet = dc_motor_position(0, pos1, 1, pos2, 8000);

	return packet;
}


packet_t *robot_rotate(robot_data_t *data, double degr)
{
	packet_t *packet = NULL;
	uint16_t pos1, pos2;

	pos1 = data->encoder_count[0];
	pos2 = data->encoder_count[1];

	calculate_rotation_offset(&pos1, &pos2, degr);
	packet = dc_motor_position(0, pos1, 1, pos2, 8000);

	return packet;
}
